#pragma once

#include <vector>
#include <CVector.h>

using namespace std;



class CMatrix
{
 public:  
  double** M;
    CMatrix() 
    {
      M = 0;
      M = new double*[4];
      for (int i=0;i<4;i++)
	M[i] = new double[4];

      for (int i=0;i<4;i++)
      for (int j=0;j<4;j++)
	M[i][j] = 0;
	
    }

    void free() {
      if (M) {
	for (int i=0;i<3;i++)
	  delete[] M[i];
	delete[] M;
	
      } 
    }
    void Mul(CMatrix o)
    {
	CMatrix temp;
	int i,j;
	for (j=0;j<4;j++)
		for (i=0;i<4;i++)
		  temp.M[i][j] = M[i][0] * o.M[0][j] + 
		                 M[i][1] * o.M[1][j] +
		                 M[i][2] * o.M[2][j] +
		                 M[i][3] * o.M[3][j] ;

	Copy(temp);
	temp.free();
    }


    void Zyz(double phi, double theta, double psi) {
         
        double  sphi, cphi, sth, cth, spsi, cpsi;

	

        sphi = sin(phi);
        cphi = cos(phi);

        sth  = sin(theta);
        cth  = cos(theta);

        spsi = sin(psi);
        cpsi = cos(psi);
        

        M[0][0] = -sphi * spsi + cth * cphi * cpsi;
        M[1][0]= -sphi * cpsi - cth * cphi * spsi;
        M[2][0] =                sth * cphi;

        M[0][1] =  cphi * spsi + cth * sphi * cpsi;
        M[1][1] =  cphi * cpsi - cth * sphi * spsi;
        M[2][1] =                sth * sphi;

        M[0][2] =              - sth * cpsi;
        M[1][2] =                sth * spsi;
        M[2][2] =                cth;

        }
      

    void Copy(CMatrix src)
    {
	int i,j;
	for (i=0;i<4;i++)
		for (j=0;j<4;j++)
                  M[i][j] = src.M[i][j];
    }
    
    void Identity()
    {
	M[0][0] = 1; M[0][1] = 0; M[0][2] = 0;  M[0][3] = 0;
	M[1][0] = 0; M[1][1] = 1; M[1][2] = 0;  M[1][3] = 0;
	M[2][0] = 0; M[2][1] = 0; M[2][2] = 1;  M[2][3] = 0;
	M[3][0] = 0; M[3][1] = 0; M[3][2] = 0;  M[3][3] = 1;
    }

    void RotateXY(double angle)
    {
	double c, s;
	c = cos(angle);
	s = sin(angle);
	Identity();
	M[0][0] = c; M[0][1] = s;  M[0][2] = 0; 
	M[1][0] =-s; M[1][1] = c;  M[1][2] = 0; 
	M[2][0] = 0; M[2][1] = 0;  M[2][2] = 1; 
    }

    void RotateYZ(double angle)
    {
	double c, s;
	c = cos(angle);
	s = sin(angle);
	Identity();
	M[0][0] = 1; M[0][1] = 0;  M[0][2] = 0; 
	M[1][0] = 0; M[1][1] = c;  M[1][2] = s; 
	M[2][0] = 0; M[2][1] =-s;  M[2][2] = c; 
    }
    
    void RotateXZ(double angle)
    {
	double c, s;
	c = cos(angle);
	s = sin(angle);
	Identity();
	M[0][0] = c; M[0][1] = 0;  M[0][2] =-s; 
	M[1][0] = 0; M[1][1] = 1;  M[1][2] = 0; 
	M[2][0] = s; M[2][1] = 0;  M[2][2] = c; 
    }

    void viewMatrix(CVector camera, CVector target, CVector up )
    {
      
      CVector f = (target-camera).Normalize();
      CVector s = f.Cross(up);
      CVector u = s.Cross(f);

      
      CMatrix m2;
      Identity();
      m2.Identity();
      for(int i=0;i<3;i++){
        M[0][i] = s[i];
        M[1][i] = u[i];
        M[2][i] = -f[i];

        m2.M[i][3]=-camera[i];
        m2.M[i][3]=-camera[i];
        m2.M[i][3]=-camera[i];
      }

      Mul(m2);

}


    inline CVector Mul(CVector v) {
      double xt = M[0][0] * v.x + M[1][0] * v.y + M[2][0] * v.z; 
      double yt = M[0][1] * v.x + M[1][1] * v.y + M[2][1] * v.z; 
      double zt = M[0][2] * v.x + M[1][2] * v.y + M[2][2] * v.z; 
      return CVector(xt,yt,zt);
  }

    void Print() {
      cout << endl << endl;
      for (int i=0;i<4;i++) {
	for (int j=0;j<4;j++) {
	  cout << M[j][i] << "   ";
	  }
      cout << endl;
      }
    }

};


